
#include "full_cover_path_planner/cell_decomposer.h"
#include "full_cover_path_planner/bcd_decomposer.h"
#include "glog_func/log.h"

#include <ros/ros.h>

#include <opencv2/opencv.hpp>
#include <unordered_set>
#include <CGAL/Polygon_vertical_decomposition_2.h>

namespace global_path_planner
{
    namespace ccpp
    {

        CellDecomposer::CellDecomposer(std::shared_ptr<Polygon2D>& poly2d)
                        :__polygon2d_Ptr(poly2d)
        {
            __debug_Cell = ros::param::param<bool>("/full_cover_path/debug_cell", false);
            __min_Polygon_Area = ros::param::param<float>("/full_cover_path/min_polygon_area", 4.0);
            __bcd_Decomp_Ptr = std::make_shared<BcdDecomposer>(BcdDecomposer(__polygon2d_Ptr));
        }

        CellDecomposer::~CellDecomposer()
        {

        }

        // 创建std::vector<PolygonWithHoles>
        std::vector<PolygonWithHoles> CellDecomposer::CreatePolygonWithHoles(std::vector<std::vector<cv::Point>> polys)
        {
            // 构建多边形类（包括障碍物/虚拟墙等）
            std::vector<cv::Point> outer_poly = polys.front();
            polys.erase(polys.begin());
            std::vector<std::vector<cv::Point>> inner_polys = polys;

            Polygon_2 outer_polygon;
            for (const auto &point : outer_poly)
            {
                outer_polygon.push_back(Point_2(point.x, point.y));
            }

            // 判断最外层多边形模型是否交叉点
            std::vector<Polygon_2> res_out_polys, not_simple_polys;
            not_simple_polys.emplace_back(outer_polygon);
            res_out_polys = __polygon2d_Ptr->GenSimplePolys(not_simple_polys, true);

            // 构建内部障碍物模型
            int num_holes = inner_polys.size();
            std::vector<Polygon_2> hole_polys(num_holes);
            for (int i = 0; i < inner_polys.size(); i++)
            {
                for (const auto &point : inner_polys[i])
                {
                    hole_polys[i].push_back(Point_2(point.x, point.y));
                }
            }

            // 判断内部障碍物是否交叉点
            std::vector<Polygon_2> res_hole_polys;
            res_hole_polys = __polygon2d_Ptr->GenSimplePolys(hole_polys, false);

            // 判断子轮廓位于哪个外围轮廓中
            std::map<int, std::vector<Polygon_2>> holes_map;
            std::unordered_set<int> remove_idx;
            for (auto i = 0; i < res_out_polys.size(); i++)
            {
                std::vector<Polygon_2> tmp_holes;
                // PolygonWithHoles tmp_pwh(res_out_polys[i]);
                for (int j = 0; j < res_hole_polys.size(); j++)
                {
                    LOG(INFO) << "check pwh (" << i << ", " << j << ")";
                    if (remove_idx.find(j) != remove_idx.end()) {
                        continue;
                    }
                    for (auto it = res_hole_polys[j].vertices_begin();
                         it != res_hole_polys[j].vertices_end(); it++)
                    {
                        if (__polygon2d_Ptr->pointInPolygon(res_out_polys[i], *it))
                        {
                            remove_idx.insert(j);
                            tmp_holes.emplace_back(res_hole_polys[j]);
                            LOG(INFO) << j << " is in " << i;
                            break;
                        }
                    }
                }
                holes_map[i] = tmp_holes;
            }

            // 对于每个simple polygon处理
            std::vector<PolygonWithHoles> res_poly_with_holes;
            for (auto i = 0; i < res_out_polys.size(); i++)
            {
                if (res_out_polys[i].size() <= 2) {
                    LOG(WARNING) << "Invalid out polygon";
                    continue;
                }
                PolygonWithHoles pwh(res_out_polys[i], holes_map[i].begin(), holes_map[i].end());
                __bcd_Decomp_Ptr->simplifyPolygonWithHoles(&pwh); // 移除共线的点
                auto p_pwh = removeEdgesMoreThanPoints(pwh); // TODO: rj.wang
                if (!p_pwh.first) {
                    LOG(INFO) << "removeEdgesMoreThanPoints false";
                    continue;
                }
                else {
                    __bcd_Decomp_Ptr->simplifyPolygonWithHoles(&p_pwh.second);
                }
                if (!__bcd_Decomp_Ptr->isStrictlySimple(p_pwh.second)) {
                    LOG(WARNING) << "pwh not strictly simple";
                } else {
                    res_poly_with_holes.emplace_back(p_pwh.second);
                }
            }
            return res_poly_with_holes;
        }

        // 移除多边形非相邻边有交点
        std::pair<bool, PolygonWithHoles> CellDecomposer::removeEdgesMoreThanPoints(PolygonWithHoles &pwh)
        {
            // result hole Polygon
            std::vector<Polygon_2> res_hole_polys;
            // 从大到小排序的hole
            std::vector<Polygon_2> sort_hole_polys;
            // result outer Polygon
            Polygon_2 res_out_polys = pwh.outer_boundary();
            // 外围轮廓移除共线的点
            __polygon2d_Ptr->removeCollinearVertices(&res_out_polys);
            if (res_out_polys.size() <= 2)
                return std::make_pair(false, PolygonWithHoles());

            // 计算多边形最外层边界与内部障碍物是否交叉
            for (auto it = pwh.holes_begin(); it != pwh.holes_end(); it++)
            {
                // if (!__polygon2d_Ptr->IsClockwise(res_out_polys))
                if(CGAL::to_double(res_out_polys.area()) > 0)
                    res_out_polys.reverse_orientation();
                // 合并out_boundry与hole
                Polygon_2 hole_it = *it;
                __polygon2d_Ptr->removeCollinearVertices(&hole_it);
                if (hole_it.size() <= 0)
                    continue;
                // if (!__polygon2d_Ptr->IsClockwise(hole_it))
                if(CGAL::to_double(hole_it.area()) > 0)
                    hole_it.reverse_orientation();
                auto new_poly = __polygon2d_Ptr->MergeIntersectionPolygons(res_out_polys, hole_it);
                if (new_poly.is_empty())
                {
                    sort_hole_polys.emplace_back(hole_it);
                }
                else
                {
                    res_out_polys = new_poly;
                }
            }

            // 合并后外围轮廓移除共线的点
            __polygon2d_Ptr->removeCollinearVertices(&res_out_polys);
            if (res_out_polys.size() <= 2)
                return std::make_pair(false, PolygonWithHoles());

            // 根据轮廓面积进行排序
            auto poly_2d = __polygon2d_Ptr;
            std::vector<int> hole_indices(sort_hole_polys.size());
            std::iota(hole_indices.begin(), hole_indices.end(), 0);
            std::sort(hole_indices.begin(), hole_indices.end(),
                        [&sort_hole_polys, poly_2d](int lhs, int rhs)
                        { return poly_2d->computeArea(sort_hole_polys[lhs]) >
                               poly_2d->computeArea(sort_hole_polys[rhs]); });
            // 合并内部Polygon
            std::unordered_set<int> remove_hole_index;
            for (auto i = 0; i < hole_indices.size();)
            {
                Polygon_2 cur_poly = sort_hole_polys[hole_indices[i]];
                __polygon2d_Ptr->removeCollinearVertices(&cur_poly);
                if (cur_poly.size() <= 0)
                    continue;
                // if (!__polygon2d_Ptr->IsClockwise(cur_poly))
                if(CGAL::to_double(cur_poly.area()) > 0)
                    cur_poly.reverse_orientation();
                for (auto j = i + 1; j < hole_indices.size(); j++)
                {
                    Polygon_2 next_poly = sort_hole_polys[hole_indices[j]];
                    __polygon2d_Ptr->removeCollinearVertices(&next_poly);
                    if (next_poly.size() <= 0)
                        continue;
                    // if (!__polygon2d_Ptr->IsClockwise(next_poly))
                    if(CGAL::to_double(next_poly.area()) > 0)
                        next_poly.reverse_orientation();
                    auto new_poly = __polygon2d_Ptr->MergeIntersectionPolygons(cur_poly, next_poly);
                    if (!new_poly.is_empty())
                    {
                        remove_hole_index.insert(j);
                        cur_poly = new_poly;
                    }
                }
                res_hole_polys.emplace_back(cur_poly);
                i++;
                while (remove_hole_index.find(i) != remove_hole_index.end())
                {
                    i++;
                }
            }
            // result PolygonWithHoles
            PolygonWithHoles res_pwh(res_out_polys, res_hole_polys.begin(), res_hole_polys.end());
            return std::make_pair(true, res_pwh);
        }

        // 分解多边形生成单个清扫轮廓
        std::vector<Polygon_2> CellDecomposer::DecomposePolygons(std::vector<PolygonWithHoles> &pwhs, cv::Mat& img)
        {
            bool test_gpp = ros::param::param("test_gpp", false);
            static int test_i = 0;
            std::vector<Polygon_2> bcd_cells;
            cv::Mat tmp_img;
            img.copyTo(tmp_img);
            static int id = 0;
            // bcd分解
            for (auto i = 0; i < pwhs.size(); i++)
            {
                // 轮廓的最长边最为分解单元的方向
                Direction_2 dir = findBestDirForDecomposer(pwhs[i]);

                // 调试代码
                if (test_gpp) {
                    cv::Mat white_map(img.rows, img.cols, CV_8UC1, cv::Scalar(255));
                    DrawPolygonOnMap(pwhs[i].outer_boundary(), white_map, cv::Scalar(0));
                    for (auto hole = pwhs[i].holes_begin(); hole != pwhs[i].holes_end(); ++hole) {
                        DrawPolygonOnMap(*hole, white_map, cv::Scalar(0));
                    }
                    std::string map_file = common::log::kGlobalPathPlannerLogPath + "/" + common::log::CurTimeString() + "_" + std::to_string(test_i) + "_bcd_pwh.png";
                    cv::flip(white_map, white_map, 0);
                    cv::imwrite(map_file, white_map);
                }

                // BcdDecomposer bcd_decomposer;
                std::vector<Polygon_2> tmp_bcd_cells = __bcd_Decomp_Ptr->getPolygonsForBcd(pwhs[i], dir);
                bcd_cells.insert(bcd_cells.end(), tmp_bcd_cells.begin(), tmp_bcd_cells.end());

                // 调试代码
                if (test_gpp) {
                    cv::Mat white_map(img.rows, img.cols, CV_8UC1, cv::Scalar(255));
                    for (const auto& bcd_cell : bcd_cells) {
                        DrawPolygonOnMap(bcd_cell, white_map, cv::Scalar(0));
                    }
                    std::string map_file = common::log::kGlobalPathPlannerLogPath + "/" + common::log::CurTimeString() + "_" + std::to_string(test_i) + "_bcd_cells.png";
                    cv::flip(white_map, white_map, 0);
                    cv::imwrite(map_file, white_map);
                }

                // 调试代码
                if (test_gpp) {
                    // TCD
                    CGAL::Aff_transformation_2<K> rotation(CGAL::ROTATION, dir, 1, 1e9);
                    rotation = rotation.inverse();
                    PolygonWithHoles rotated_pwh_i = pwhs[i];
                    rotated_pwh_i.outer_boundary() =
                        CGAL::transform(rotation, pwhs[i].outer_boundary());
                    PolygonWithHoles::Hole_iterator hit_rot = rotated_pwh_i.holes_begin();
                    for (PolygonWithHoles::Hole_const_iterator hit = pwhs[i].holes_begin();
                        hit != pwhs[i].holes_end(); ++hit) {
                        *(hit_rot++) = CGAL::transform(rotation, *hit);
                    }

                    std::vector<Polygon_2> tcd_cells;
                    CGAL::Polygon_vertical_decomposition_2<K> decom;
                    decom(rotated_pwh_i, std::back_inserter(tcd_cells));
                    for (auto& p : tcd_cells) {
                        CGAL::Aff_transformation_2<K> rotation_tmp(CGAL::ROTATION, dir, 1, 1e9);
                        p = CGAL::transform(rotation_tmp, p);
                    }

                    cv::Mat white_map(img.rows, img.cols, CV_8UC1, cv::Scalar(255));
                    for (const auto& tcd_cell : tcd_cells) {
                        DrawPolygonOnMap(tcd_cell, white_map, cv::Scalar(0));
                    }
                    std::string map_file = common::log::kGlobalPathPlannerLogPath + "/" + common::log::CurTimeString() + "_" + std::to_string(test_i) + "_tcd_cells.png";
                    cv::flip(white_map, white_map, 0);
                    cv::imwrite(map_file, white_map);
                }
            }
            if(bcd_cells.size() <= 0) return bcd_cells;

            // 根据最小外接矩形最长边之间的夹角判断两者是否合并
            std::vector<Polygon_2> merge_bcd_cells;
            #if 1
            std::set<int> idx_exist_sets; // 记录已经合并的索引
            if(bcd_cells.size() == 1){
                merge_bcd_cells = bcd_cells;
                idx_exist_sets.insert(0);
            }else if(bcd_cells.size() > 1){
                for(auto i = 0; i < bcd_cells.size(); i++){
                    if(idx_exist_sets.find(i) != idx_exist_sets.end()) continue;
                    auto cur_cell_poly = bcd_cells[i];
                    __polygon2d_Ptr->removeCollinearVertices(&cur_cell_poly);
                    if (CGAL::to_double(cur_cell_poly.area()) > 0)
                        cur_cell_poly.reverse_orientation();
                    bool is_merge_poly = false;
                    auto longest1_pts = getlongestEdge(cur_cell_poly);

                    for(auto j = 0; j < bcd_cells.size(); j++){
                        if(idx_exist_sets.find(j) != idx_exist_sets.end() || j == i) continue;
                        auto next_cell_poly = bcd_cells[j];
                        __polygon2d_Ptr->removeCollinearVertices(&next_cell_poly);
                        if (CGAL::to_double(next_cell_poly.area()) > 0)
                            next_cell_poly.reverse_orientation();
                        auto longest2_pts = getlongestEdge(next_cell_poly);
                        Polygon_2 merge_poly;
                        merge_poly = __polygon2d_Ptr->MergeIntersectionPolygons(cur_cell_poly, next_cell_poly, false);
                        if(merge_poly.is_empty()) continue;
                        else{
                            auto dx1 = std::get<0>(longest1_pts).x -  std::get<1>(longest1_pts).x;
                            auto dy1 = std::get<0>(longest1_pts).y - std::get<1>(longest1_pts).y;
                            auto dx2 = std::get<0>(longest2_pts).x - std::get<1>(longest2_pts).x;
                            auto dy2 = std::get<0>(longest2_pts).y - std::get<1>(longest2_pts).y;
                            auto costheta = (dx1 * dx2 + dy1 * dy2) /
                                            (std::sqrt(dx1 * dx1 + dy1 * dy1) *
                                            std::sqrt(dx2 * dx2 + dy2 * dy2));
                            if(std::fabs(costheta) > 0.9)  // 两条直线合并的判断条件
                            {
                                is_merge_poly = true;
                                cur_cell_poly = merge_poly;
                                idx_exist_sets.insert(j);
                                if (CGAL::to_double(cur_cell_poly.area()) > 0)
                                    cur_cell_poly.reverse_orientation();
                            }
                        }
                    }
                    if(is_merge_poly && std::get<2>(getlongestEdge(cur_cell_poly)) > __min_Polygon_Area) { // TODO: rj.wang 要求长度大于4个栅格
                        idx_exist_sets.insert(i);
                        merge_bcd_cells.emplace_back(cur_cell_poly);
                    }
                }
            }
            // 未融合的将合并到merge中
            for(auto i = 0; i < bcd_cells.size(); i++){
                if(idx_exist_sets.find(i) == idx_exist_sets.end() &&
                    std::get<2>(getlongestEdge(bcd_cells[i])) > __min_Polygon_Area){ // 丢弃部分cell
                    merge_bcd_cells.emplace_back(bcd_cells[i]);
                }
            }
            LOG(INFO) << "bcd_cells.size " << bcd_cells.size() << ", merge_bcd_cells.size " << merge_bcd_cells.size();

            // 调试代码
            if (test_gpp) {
                cv::Mat white_map(img.rows, img.cols, CV_8UC1, cv::Scalar(255));
                for (const auto& bcd_cell : merge_bcd_cells) {
                    DrawPolygonOnMap(bcd_cell, white_map, cv::Scalar(0));
                }
                std::string map_file = common::log::kGlobalPathPlannerLogPath + "/" + common::log::CurTimeString() + "_bcd_merged_cells.png";
                cv::flip(white_map, white_map, 0);
                cv::imwrite(map_file, white_map);
            }
            #endif
            test_i ++;

            return merge_bcd_cells;
        }

        // bcd分解cell
        // bool CellDecomposer::computeBestBCDFromPolygonWithHoles(const PolygonWithHoles &pwh,
        //                                                         std::vector<Polygon_2> *bcd_polygons)
        // {
        //     // 轮廓的最长边最为分解单元的方向
        //     Direction_2 dir = findBestDirForDecomposer(pwh);
        //     // BcdDecomposer bcd_decomposer;
        //     std::vector<Polygon_2> cells = __bcd_Decomp_Ptr->getPolygonsForBcd(pwh, dir);
        //     *bcd_polygons = cells;
        //     if (bcd_polygons->empty())
        //         return false;
        //     else
        //         return true;
        // }

        // 获取最小外接矩形的边
        std::tuple<cv::Point2f, cv::Point2f, double> CellDecomposer::getlongestEdge(Polygon_2& poly){
            std::vector<cv::Point> cv_pts;
            for(auto it = poly.vertices_begin(); it != poly.vertices_end(); it++){
                cv_pts.emplace_back(cv::Point(CGAL::to_double(it->x()), CGAL::to_double(it->y())));
            }
            cv::Point2f rect[4];
            std::tuple<cv::Point2f, cv::Point2f, double> longest_pts;
            // std::pair<cv::Point2f, cv::Point2f> shortest_pts;
            double min_dist = std::numeric_limits<double>::min();
            double max_dist = std::numeric_limits<double>::max();
            auto min_rectangle = cv::minAreaRect(cv::Mat(cv_pts));  //计算每个轮廓最小外接矩形
            min_rectangle.points(rect);  //把最小外接矩形四个端点复制给rect数组
            for(auto j = 0; j < 3; j++){
                auto dx = rect[j].x - rect[j + 1].x;
                auto dy = rect[j].y - rect[j + 1].y;
                auto lenth = std::sqrt(dx * dx + dy * dy);
                if(lenth > min_dist){
                    min_dist = lenth;
                    std::get<0>(longest_pts) = rect[j];
                    std::get<1>(longest_pts) = rect[j + 1];
                }
                if(lenth < max_dist){
                    max_dist = lenth;
                    std::get<2>(longest_pts) = lenth;
                }
            }

            return longest_pts;
        }

        // 针对多边形两条边之间的夹角过小，投影到长边坐标
        Polygon_2 CellDecomposer::CorrectPolygon(Polygon_2& origin_poly){
            Polygon_2 res_poly = origin_poly;
            Polygon_2 correct_poly;

            for(auto i = 1; i < res_poly.size(); i++){
                auto p0 = res_poly[i - 1];
                auto p1 = res_poly[i];
                auto p2 = res_poly[(i + 1) % res_poly.size()];
                auto dx1 = CGAL::to_double(p0.x()) -  CGAL::to_double(p1.x());
                auto dy1 = CGAL::to_double(p0.y()) -  CGAL::to_double(p1.y());
                auto dx2 = CGAL::to_double(p2.x()) -  CGAL::to_double(p1.x());
                auto dy2 = CGAL::to_double(p2.y()) -  CGAL::to_double(p1.y());
                auto d_p_0_1 = std::sqrt(dx1 * dx1 + dy1 * dy1);
                auto d_p_1_2 = std::sqrt(dx2 * dx2 + dy2 * dy2);
                auto costheta = (dx1 * dx2 + dy1 * dy2) /
                                (d_p_0_1 * d_p_1_2);

                if(costheta > 0.95){
                    double dx = 0., dy = 0.;
                    if(d_p_0_1 > d_p_1_2){
                        dx = (dx1 * dx2 + dy1 * dy2) / (d_p_0_1 * d_p_0_1) * dx1;
                        dy = (dx1 * dx2 + dy1 * dy2) / (d_p_0_1 * d_p_0_1) * dy1;
                    }else{
                       dx = (dx1 * dx2 + dy1 * dy2) / (d_p_1_2 * d_p_1_2) * dx2;
                       dy = (dx1 * dx2 + dy1 * dy2) / (d_p_1_2 * d_p_1_2) * dy2;
                    }
                    auto x = CGAL::to_double(p1.x()) + dx;
                    auto y = CGAL::to_double(p1.y()) + dy;
                    res_poly.set(res_poly.vertices_begin() + i, Point_2(x, y));
                }
                else if(costheta < -0.9){
                    auto x = (CGAL::to_double(p0.x()) + CGAL::to_double(p2.x())) / 2.0;
                    auto y = (CGAL::to_double(p0.y()) + CGAL::to_double(p2.y())) / 2.0;
                    res_poly.set(res_poly.vertices_begin() + i, Point_2(x, y));
                }
            }

            return res_poly;
        }

        // 最长边作为最佳分解单元
        Direction_2 CellDecomposer::findBestDirForDecomposer(const PolygonWithHoles &pwh)
        {
            // Get all sweepable edges.
            std::vector<Direction_2> edge_dirs = findAllEdgeDirections(pwh);
            for (auto &d : edge_dirs)
            {
                d = Direction_2(-d.dy(), d.dx());
            }
            Direction_2 longest_dir;
            // Find max distance.
            double max_dist = std::numeric_limits<double>::min();
            for (const auto &dir : edge_dirs)
            {
                auto vect = dir.vector();
                double dist = CGAL::to_double(vect.squared_length());
                if (dist > max_dist)
                {
                    max_dist = dist;
                    longest_dir = dir;
                }
            }
            return longest_dir;
        }

        // 垂直当前边的方向
        std::vector<Direction_2> CellDecomposer::findPerpEdgeDirections(const PolygonWithHoles &pwh)
        {
            std::vector<Direction_2> directions;
            Polygon_2 out_bound = pwh.outer_boundary();
            for(auto it = out_bound.vertices_begin(); it != out_bound.vertices_end(); it++){
                auto proj_pt = __polygon2d_Ptr->projectOnPolygon2(out_bound, *it);
                auto dx = CGAL::to_double(it->x()) - CGAL::to_double(proj_pt.second.x());
                auto dy = CGAL::to_double(it->y()) - CGAL::to_double(proj_pt.second.y());
                directions.emplace_back(Direction_2(dx, dy));
            }
            // std::vector<Direction_2> directions = findAllEdgeDirections(pwh);
            // for (auto &d : directions)
            // {
            //     d = Direction_2(-d.dy(), d.dx());
            // }
            return directions;
        }

        // 获取所有边的方向
        std::vector<Direction_2> CellDecomposer::findAllEdgeDirections(const PolygonWithHoles &pwh)
        {
            // Get all possible polygon directions.
            std::vector<Direction_2> directions;
            auto dirs = __polygon2d_Ptr->getPolygonEdgeDirections(pwh.outer_boundary());
            directions.insert(directions.end(), dirs.begin(), dirs.end());
            for (PolygonWithHoles::Hole_const_iterator hit = pwh.holes_begin();
                 hit != pwh.holes_end(); ++hit)
            {
                auto dirs = __polygon2d_Ptr->getPolygonEdgeDirections(*hit);
                directions.insert(directions.end(), dirs.begin(), dirs.end());
            }
            // Remove redundant directions.
            std::set<size_t> to_remove;
            for (size_t i = 0; i < directions.size() - 1; ++i)
            {
                for (size_t j = i + 1; j < directions.size(); ++j)
                {
                    if (CGAL::orientation(directions[i].vector(), directions[j].vector()) ==
                        CGAL::COLLINEAR)
                        to_remove.insert(j);
                }
            }
            for (std::set<size_t>::reverse_iterator rit = to_remove.rbegin();
                 rit != to_remove.rend(); ++rit)
            {
                directions.erase(std::next(directions.begin(), *rit));
            }
            // Add opposite directions.
            std::vector<Direction_2> temp_directions = directions;
            for (size_t i = 0; i < temp_directions.size(); ++i)
            {
                directions.push_back(-temp_directions[i]);
            }
            return directions;
        }

        // 多边形最长边作为最佳清扫方向
        Direction_2 CellDecomposer::findBestDirForSweep(const Polygon_2 &poly)
        {
            // Get all sweepable edges.
            std::vector<Direction_2> edge_dirs = __polygon2d_Ptr->getPolygonEdgeDirections(poly);
            Direction_2 best_dir;
            // Find max distance.
            double max_dist = std::numeric_limits<double>::min();
            for (const auto &dir : edge_dirs)
            {
                auto vect = dir.vector();
                double dist = CGAL::to_double(vect.squared_length());
                if (dist > max_dist)
                {
                    max_dist = dist;
                    best_dir = dir;
                }
            }
            return best_dir;
        }

        void CellDecomposer::DrawPolygonOnMap(const Polygon_2& polygon, const cv::Mat& cv_map, const cv::Scalar& color) const {
            Polygon_2::Vertex_circulator vc = polygon.vertices_circulator();
            do {
                cv::Point p0(CGAL::to_double(vc->x()), CGAL::to_double(vc->y()));
                cv::Point p1(CGAL::to_double(std::next(vc)->x()), CGAL::to_double(std::next(vc)->y()));
                cv::line(cv_map, p0, p1, color);
            } while (++vc != polygon.vertices_circulator());
        }

    } // namespace ccpp
} // namespace global_path_planner